#include "SerialPort.h"

void ModBusCRC16(vector<uchar>& data) {
    unsigned int i, j, tmp, CRC16;
    unsigned int len = data.size() - 2;
    CRC16 = 0xFFFF;             //CRC寄存器初始值
    for (i = 0; i < len; i++) {
        CRC16 ^= data[i];
        for (j = 0; j < 8; j++) {
            tmp = (unsigned int)(CRC16 & 0x0001);
            CRC16 >>= 1;
            if (tmp == 1) {
                CRC16 ^= 0xA001;    //异或多项式
            }
        }
    }
    //填充校验码
    data[len] = (unsigned char)(CRC16 & 0x00FF);
    data[len + 1] = (unsigned char)((CRC16 & 0xFF00) >> 8);
}

SerialPort::SerialPort(QObject* parent) : m_printMsg(true) {}

SerialPort::~SerialPort() {
    try {
        this->close();
    } catch (const MyError& e) {
        qCritical() << e.desc();
    }
}

void SerialPort::listPort(QStringList& portList) {
    QList<QSerialPortInfo>& portInfoList = QSerialPortInfo::availablePorts();
    for (int i = 0; i < portInfoList.size(); i++) {
        portList.push_back(portInfoList[i].portName());
    }
}

bool SerialPort::openPort(const QString& port, int baudRate) {
    bool status = false;
    if (m_num.isEmpty()) {
        if (!port.isEmpty()) {
            m_num = port;
        } else {
            throw MyError(MyErrorType::SERIAL_OPEN_ERROR);
        }
    }
    m_num = port;
    qInfo() << QString("正在打开串口：%1").arg(m_num);
    try {
        //打开串口
        if (m_port.isOpen()) {
            m_port.clear();
            m_port.close();
        }
        m_port.setPortName(m_num);
        status = m_port.open(QIODevice::ReadWrite);
        if (status) {
            m_port.setDataBits(QSerialPort::Data8);
            m_port.setParity(QSerialPort::NoParity);
            m_port.setStopBits(QSerialPort::OneStop);
            m_port.setFlowControl(QSerialPort::NoFlowControl);
            m_port.setBaudRate(baudRate, QSerialPort::AllDirections);
            //将读缓冲区大小设置成1024
            m_port.setReadBufferSize(1024);
            //清除缓存，方式上电时下位机发送数据
            m_port.clear();
        }
    } catch (const std::exception& e) {
        qCritical() << "串口打开失败:" << e.what();
        qCritical() << m_port.errorString();
        throw MyError(MyErrorType::SERIAL_OPEN_ERROR);
    } catch (...) {
        qCritical() << "串口打开失败，未知异常...";
        qCritical() << m_port.errorString();
        throw MyError(MyErrorType::SERIAL_OPEN_ERROR);
    }
    return status;
}

bool SerialPort::openServoPort(const QString& port, int baudRate)
{
    bool status = false;
    try {
        if (this->isOpen())
        {
            this->clear();
            this->close();
        }
        //指定要打开的端口的名称
        this->setPortName(port);
        if (this->open(QIODevice::ReadWrite))
        {
            //串口打开成功时设置串口参数
            //设置波特率和读写方向
            this->setBaudRate(QSerialPort::Baud9600, QSerialPort::AllDirections);//波特率
            this->setDataBits(QSerialPort::Data8);//数据位
            this->setFlowControl(QSerialPort::NoFlowControl);//无流控制
            this->setParity(QSerialPort::NoParity);//无校验位
            this->setStopBits(QSerialPort::OneStop);//一个停止位
            qDebug() << tr("串口打开成功!") << endl;
            status = true;
        }
        else
        {
            /*QString msg = tr("串口打开失败，没有找到串口：%1 请检线路是否插好!");
            QMessageBox::critical(nullptr, tr("错误!"), msg.arg(port_name), tr("关闭"));*/
            status = false;
        }
    }
    catch (const std::exception& e) {
       qCritical() << "串口打开失败:" << e.what();
       qCritical() << m_servo_port.errorString();
       throw MyError(MyErrorType::SERIAL_OPEN_ERROR);
    }
   catch (...) {
       qCritical() << "串口打开失败，未知异常...";
       qCritical() << m_servo_port.errorString();
       throw MyError(MyErrorType::SERIAL_OPEN_ERROR);
   }

    if (status == 1)
    {
        qDebug()<<"串口打开成功";
    }
    return status;
}

bool SerialPort::isOpen() {
    return m_port.isOpen();
}

void SerialPort::close() {
    try {
        if (m_port.isOpen()) {
            m_port.clear();
            m_port.close();
        }
    } catch (const std::exception& e) {
        qCritical() << e.what();
        throw MyError(MyErrorType::SERIAL_CLOSE_ERROR);
    } catch (...) {
        qCritical() << "串口关闭失败...";
        throw MyError(MyErrorType::SERIAL_CLOSE_ERROR);
    }
}

void SerialPort::enablePrintLogger(bool enable) {
    m_printMsg = enable;
}

void SerialPort::resetMotor()
{
    qInfo() << tr("发送复位指令.....");
    sendDataC(RESET);
}

bool SerialPort::shankeHands()
{
    PortResponse res;
    try {
        sendDataC(SHAKE_HANDS);
        res = this->readDataImmdiately();
    }
    catch (const std::exception& e)
    {
        qCritical() << e.what();
        return false;
    }
    return res.status;
}

bool SerialPort::lightControl(QByteArray& data)
{
    qInfo() << tr("发送光源控制指令.....");
    PortResponse res;
    try {
        
        LIGHT_CONTROL[9] = data[0];
        QByteArray lightControlByteArr;
        vector2ByteArray(LIGHT_CONTROL, lightControlByteArr);
        fillCheckCode(lightControlByteArr);
        res = this->readDataImmdiately();
    }
    catch (const std::exception& e)
    {
        qCritical() << e.what();
        return false;
    }
    return res.status;
}

bool SerialPort::laserControl(QByteArray& data)
{
    qInfo() << tr("发送激光器控制指令.....");
    PortResponse res;
    try {
        LASER_CONTROL[9] = data[0];
        QByteArray laserControlByteArr;
        vector2ByteArray(LASER_CONTROL, laserControlByteArr);
        fillCheckCode(laserControlByteArr);
        res = this->readDataImmdiately();
    }
    catch (const std::exception& e)
    {
        qCritical() << e.what();
        return false;
    }
    return res.status;
}

bool SerialPort::activeWheelControl(QByteArray& data)
{
    qInfo()<<tr("发送主动轮控制指令.....");
    PortResponse res;
    try {
        ACTIVE_WHEEL_CONTROL[9] = data[0];
        QByteArray activeWheelControlByteArr;
        vector2ByteArray(ACTIVE_WHEEL_CONTROL, activeWheelControlByteArr);
        fillCheckCode(activeWheelControlByteArr);
        res = this->readDataImmdiately();
    }
    catch (const std::exception& e)
    {
        qCritical() << e.what();
        return false;
    }
    return res.status;
}

bool SerialPort::passiveWheelControl(QByteArray& data)
{
    qInfo() << tr("发送被动轮控制指令.....");
    PortResponse res;
    try {
        PASSIVE_WHEEL_CONTROL[9] = data[0];
        QByteArray passiveWheelControlByteArr;
        vector2ByteArray(PASSIVE_WHEEL_CONTROL, passiveWheelControlByteArr);
        fillCheckCode(passiveWheelControlByteArr);
        res = this->readDataImmdiately();
    }
    catch (const std::exception& e)
    {
        qCritical() << e.what();
        return false;
    }
    return res.status;
}

bool SerialPort::servoControl(QByteArray& data)
{
    qInfo() << tr("发送伺服控制指令.....");
    PortResponse res;
    try {
        if (data.size() == 2)//两个数据位
        {
            SERVO_CONTROL[9] = data[0];
            SERVO_CONTROL[10]= data[1];
            QByteArray servoControlByteArr;
            vector2ByteArray(SERVO_CONTROL, servoControlByteArr);
            fillCheckCode(servoControlByteArr);
            sendDataB(servoControlByteArr);
            res = this->readDataImmdiately();
        }
    }
    catch (const std::exception& e)
    {
        qCritical() << e.what();
        return false;
    }
    return res.status;
}

void SerialPort::sendData(const vector<uchar>& data) {
    if (m_port.isOpen() && data.size() > 0) {
        QThread::msleep(50);
        //复制数据
        QByteArray buf;
        for (uchar val : data) { buf.push_back(val); }
        //发送数据
        try {
            m_port.write(buf);
            m_port.flush();
            if (m_printMsg) { qInfo() << "send: " << buf.toHex(' '); }
        } catch (const exception& e) {
            qCritical() << e.what();
            qCritical() << m_port.errorString();
            throw MyError(MyErrorType::SERIAL_WRITE_ERROR);
        } catch (...) {
            qCritical() << "发送指令异常";
            qCritical() << m_port.errorString();
            throw MyError(MyErrorType::SERIAL_WRITE_ERROR);
        }
    }
}

unsigned char* SerialPort::floatToBytes(float value)
{
    static unsigned char bytes[4];
    std::memcpy(bytes, &value, sizeof(float));
    return bytes;
}

uint16_t SerialPort::crc16_ccitt(const std::vector<unsigned char>& data)
{
    uint16_t crc = 0xFFFF;
    for (size_t i = 0; i < data.size(); ++i) {
        crc ^= data[i] << 8;
        for (int j = 0; j < 8; ++j) {
            if (crc & 0x8000) {
                crc = (crc << 1) ^ 0x1021; // CRC-16-CCITT 多项式
            }
            else {
                crc <<= 1;
            }
        }
    }
    return crc;
}

ushort SerialPort::calculataCheckCode(QByteArray& data)
{
    if (data.size() == MIN_DATA_LEN)
    {
        ushort code = ushort(0x30);//ushort 两个字节，存储两个字节的校验码
        code <<= 8;//左移8位
        code += ushort(0x30);
        return code;
    }
    else
    {
        ushort code = 0;
        for (int i = 2; i < data.size() - 5; i++)//从功能码位开始 02 ff 31 30
        {
            code += static_cast<uchar>(data[i]);
        }
        code = ~code;
        code += 1;
        return code;
    }
}

bool SerialPort::compareCheckCode(QByteArray& data)
{
    //判断有无数据部分，若没有数据部分，校验码全为0x30,否则就计算校验码
    if (data.size() == MIN_DATA_LEN)
    {
        return (0x30 == data[9] && 0x30 == data[10]);
    }
    else
    {
        ushort check_code = calculataCheckCode(data);
        ushort origin_code1 = data[data.size() - 5] & 0xff;//取后八位
        ushort origin_code2 = data[data.size() - 4] & 0xff;
        ushort origin_code = origin_code1 << 8;
        origin_code += origin_code2;
        return check_code == origin_code;
    }
}

void SerialPort::fillCheckCode(QByteArray& data)
{
    ushort check_code = calculataCheckCode(data);
    uchar cc1 = check_code >> 8;
    uchar cc2 = check_code;
    data[data.size() - 5] = cc1;
    data[data.size() - 4] = cc2;
}

PortResponse SerialPort::readDataImmdiately(int timeout)
{
    PortResponse res;
    try {
        //每次发送指令后等待数据传回的时间
        int allWaitTime = 0;
        //当传回的数据量不够时等待的时间，毫秒为单位
        int waitForReadyReadTime = 100;
        //当等待数据到达的时间小于timeout的时候，不断循环去读取数据
        QByteArray dataBuff;
        while (allWaitTime <= timeout) {
            if (this->bytesAvailable() > 0) {
                dataBuff.append(this->read(1));
            }
            else {
                this->waitForReadyRead(waitForReadyReadTime);
                allWaitTime += waitForReadyReadTime;
                continue;
            }
            //查询结束符
            int end = dataBuff.indexOf("\r\n");
            if (end > -1) {
                end += 2;
                //从dataBuff中查询帧头
                int begin = dataBuff.indexOf(0x02);
                if (begin > -1 && begin < end) {
                    //应该截取的数据长度
                    int len = end - begin;
                    if (len > 0) {
                        //截取结束符之前的数据
                        QByteArray tempData = dataBuff.mid(begin, len);
                        QByteArray data = tempData.mid(begin);
                        qDebug() << "receive: " << data.toHex(' ') << "\n";
                        //解析数据包
                        // receive:  "02 ff 30 3a 30 30 31 01 ff 04 03 0d 0a"
                        res = parseData(data);
                    }
                }
                //清除串口缓冲区中的数据, 退出循环
                this->clear();
                break;
            }
        }
        if (allWaitTime > timeout) {
            qDebug() << "allWaitTime=" << allWaitTime;
            qDebug() << "timeout=" << timeout;
            throw exception("read data timeout...");
        }
    }
    catch (const std::exception& e) {
        throw e;
    }
    return res;
}

PortResponse SerialPort::parseData(QByteArray& data)
{
    PortResponse result;
    //先判断数据长度是否满足一帧的最小长度和计算校验码是否正确
    if (data.startsWith(0x02) && data.endsWith("\r\n")) {
        //获取功能码
        QByteArray functionCode;
        functionCode.push_back(data[4]);
        functionCode.push_back(data[5]);
        if (functionCode != "ER") {
            result.code = functionCode.toInt();
        }
        else {
            result.code = 500;
        }
        //计算数据长度
        int len = (data[6] - 0x30) * 100 + (data[7] - 0x30) * 10 + (data[8] - 0x30);
        //若数据长度大于0，获取数据部分
        if (len > 0) {
            QByteArray buf;
            for (int i = 0; i < len; i++) {
                buf.push_back(data[9 + i]);
            }
            result.data = buf;
        }
        result.status = true;
    }

    qDebug() << "Result Data: " << result.data.toHex(' ');
    return result;
}

void SerialPort::sendDataC(vector<uchar>& data)
{
    QByteArray send_buf;
    for (int i = 0; i < data.size(); i++)
    {
        send_buf.push_back(data[i]);
    }
    sendDataB(send_buf);
}

void SerialPort::sendDataB(const QByteArray& data)
{
    //休眠100ms之后再发送数据
    QThread::msleep(100);
    //发送数据时需要上锁，防止多线程同时发送数据
    QMutexLocker locker(&mutex);
    qDebug() << "发送数据为:" << data.toHex(' ') << endl;
    //发送指令给下位机
    this->write(data);
    this->flush();
    if (!this->waitForBytesWritten())
    {
        qDebug() << "发送数据出错！" << endl;
    }
}

void SerialPort::vector2ByteArray(vector<uchar>& data, QByteArray& buffer)
{
    for (int i = 0; i < data.size(); i++)
    {
        buffer.push_back(data[i]);
    }
}

//////////////////////////////////////////////////////////////////////////
///                            RS485Port                               ///
//////////////////////////////////////////////////////////////////////////


RS485Port::RS485Port(QObject* parent) : SerialPort(parent) {}

void RS485Port::initRunModel() {
    qInfo() << "初始化运动模式...";
    try {
        //设置运动模式
        vector<uchar> data = { 0x02, 0x06, 0x00, 0x00, 0x03, 0x02, 0x08, 0xC8 };
        sendData(data);
        readData(8);
    } catch (const MyError& e) {
        qCritical() << e.desc();
        throw e;
    } catch (const exception& e) {
        qCritical() << "初始化运动模式失败:" << e.what();
        qCritical() << m_port.errorString();
        throw MyError(MyErrorType::INIT_MOTO_RUNMODEL_ERROR);
    } catch (...) {
        qCritical() << "初始化运动模式失败...";
        qCritical() << m_port.errorString();
        throw MyError(MyErrorType::INIT_MOTO_RUNMODEL_ERROR);
    }
}

void RS485Port::set3and4inputMode()
{
    qInfo() << "设置传感器3和4输入模式模式...";
    try {
        //设置传感器3和4输入模式
        vector<uchar> data = { 0x02, 0x06, 0x00, 0x0F, 0x00, 0x00, 0xB9, 0xFA };
        sendData(data);
        readData(8);
    }
    catch (const MyError& e) {
        qCritical() << e.desc();
        throw e;
    }
    catch (const exception& e) {
        qCritical() << "设置传感器3和4输入模式失败:" << e.what();
        qCritical() << m_port.errorString();
        throw MyError(MyErrorType::INIT_MOTO_RUNMODEL_ERROR);
    }
    catch (...) {
        qCritical() << "设置传感器3和4输入模式失败...";
        qCritical() << m_port.errorString();
        throw MyError(MyErrorType::INIT_MOTO_RUNMODEL_ERROR);
    }
}

void RS485Port::setSensorInternalPullUp()
{
    qInfo() << "设置传感器3和4上拉模式...";
    try {
        //设置传感器3和4输入模式
        vector<uchar> data = { 0x02, 0x06, 0x00, 0x2A, 0x00, 0x0C, 0xA8, 0x34 };
        sendData(data);
        readData(8);
    }
    catch (const MyError& e) {
        qCritical() << e.desc();
        throw e;
    }
    catch (const exception& e) {
        qCritical() << "设设置传感器3和4上拉模式失败:" << e.what();
        qCritical() << m_port.errorString();
        throw MyError(MyErrorType::INIT_MOTO_RUNMODEL_ERROR);
    }
    catch (...) {
        qCritical() << "设置传感器3和4上拉模式失败...";
        qCritical() << m_port.errorString();
        throw MyError(MyErrorType::INIT_MOTO_RUNMODEL_ERROR);
    }
}

void RS485Port::sensor3FallingEdgeDecelerateStop()
{
    qInfo() << "设置传感器3下降沿立刻停止...";
    try {
        //设置传感器3和4输入模式
        vector<uchar> data = { 0x02, 0x06, 0x00, 0x12, 0x02, 0x00, 0x28, 0x9C };
        sendData(data);
        readData(8);
    }
    catch (const MyError& e) {
        qCritical() << e.desc();
        throw e;
    }
    catch (const exception& e) {
        qCritical() << "设置传感器3下降沿立刻停止失败:" << e.what();
        qCritical() << m_port.errorString();
        throw MyError(MyErrorType::INIT_MOTO_RUNMODEL_ERROR);
    }
    catch (...) {
        qCritical() << "设置传感器3下降沿立刻停止失败...";
        qCritical() << m_port.errorString();
        throw MyError(MyErrorType::INIT_MOTO_RUNMODEL_ERROR);
    }
}

void RS485Port::sensor4FallingEdgeDecelerateStop()
{
    qInfo() << "设置传感器4下降沿立刻停止...";
    try {
        //设置传感器3和4输入模式
        vector<uchar> data = { 0x02, 0x06, 0x00, 0x13, 0x02, 0x00, 0x79, 0x5C };
        sendData(data);
        readData(8);
    }
    catch (const MyError& e) {
        qCritical() << e.desc();
        throw e;
    }
    catch (const exception& e) {
        qCritical() << "设置传感器4下降沿立刻停止失败:" << e.what();
        qCritical() << m_port.errorString();
        throw MyError(MyErrorType::INIT_MOTO_RUNMODEL_ERROR);
    }
    catch (...) {
        qCritical() << "设置传感器4下降沿立刻停止失败...";
        qCritical() << m_port.errorString();
        throw MyError(MyErrorType::INIT_MOTO_RUNMODEL_ERROR);
    }
}

void RS485Port::updateParams(const SerialPortConfig& config) {
    try {
        //设置加速度
        if (config.accelerated > 0) {
            qInfo() << QString("步进电机参数设置[加速度：%1]").arg(config.accelerated);
            //02 10 00 05 00 02 04 80 00 47 BB 66 97
            float value = float(config.accelerated);
            unsigned char* hexVal = floatToBytes(value);
            vector<uchar> data = { 0x02, 0x10, 0x00, 0x05, 0x00, 0x02, 0x04,  hexVal[1], hexVal[0], hexVal[3], hexVal[2], 0x0F, 0x7D };
            // 输出字节表示
            std::cout << std::hex;
            for (int i = 0; i < 4; ++i) {
                std::cout <<""<< std::setw(2) << std::setfill('0') << (int)hexVal[i] << "";
            }
            std::cout << std::endl;

            ModBusCRC16(data);
            sendData(data);
            readData(8);
            qInfo() << "加速度设置完成...";
        }
        //设置减速度
        if (config.retarded > 0) {
            qInfo() << QString("步进电机参数设置[减速度：%1]").arg(config.retarded);
            float value = float(config.retarded);
            unsigned char* hexVal = floatToBytes(value);
           vector<uchar> data = { 0x02, 0x10, 0x00, 0x07, 0x00, 0x02, 0x04, hexVal[1], hexVal[0], hexVal[3], hexVal[2], 0x0F, 0x7D };
            ModBusCRC16(data);
            sendData(data);
            readData(8);
            qInfo() << "减速度设置完成...";
        }
        //设置加速电流
        if (config.eQuicken > 0) {
            qInfo() << QString("步进电机参数设置[加速电流：%1]").arg(config.eQuicken);
            unsigned char* hexVal = (unsigned char*)&config.eQuicken;
            vector<uchar> data = { 0x02, 0x10, 0x00, 0x09, 0x00, 0x02, 0x04, hexVal[1], hexVal[0], hexVal[3], hexVal[2], 0x0F, 0x7D };
            ModBusCRC16(data);
            sendData(data);
            readData(8);
            qInfo() << "减速电流设置完成...";
        }
        //设置工作电流
        if (config.eWork > 0) {
            qInfo() << QString("步进电机参数设置[工作电流：%1]").arg(config.eWork);
            unsigned char* hexVal = (unsigned char*)&config.eWork;
            vector<uchar> data = { 0x02, 0x10, 0x00, 0x0B, 0x00, 0x02, 0x04, hexVal[1], hexVal[0], hexVal[3], hexVal[2], 0x0F, 0x7D };
            ModBusCRC16(data);
            sendData(data);
            readData(8);
            qInfo() << "工作电流设置完成...";
        }
        //保持电流
        if (config.eAwait > 0) {
            qInfo() << QString("步进电机参数设置[保持电流：%1]").arg(config.eAwait);
            unsigned char* hexVal = (unsigned char*)&config.eAwait;
            vector<uchar> data = { 0x02, 0x10, 0x00, 0x0D, 0x00, 0x02, 0x04, hexVal[1], hexVal[0], hexVal[3], hexVal[2], 0x0F, 0x7D };
            ModBusCRC16(data);
            sendData(data);
            readData(8);
            qInfo() << "保持电流设置完成...";
        }
        //细分
        if (config.motoMcs > -1) {
            qInfo() << QString("步进电机参数设置[细分：%1]").arg(pow(2, config.motoMcs));
            vector<uchar> data = { 0x02, 0x06, 0x00, 0x22, 0x00, uchar(config.motoMcs), 0x49, 0xA9 };
            ModBusCRC16(data);
            sendData(data);
            readData(8);
            qInfo() << "细分设置完成...";
        }
    } catch (const MyError& e) {
        qCritical() << e.desc();
        throw e;
    } catch (const exception& e) {
        qCritical() << "步进电机参数设置失败:" << e.what();
        throw MyError(MyErrorType::SET_MOTO_PARAMS_ERROR);
    } catch (...) {
        qCritical() << "步进电机参数设置失败...";
        throw MyError(MyErrorType::SET_MOTO_PARAMS_ERROR);
    }
}

void RS485Port::motorEnabling() {
    qInfo() << "电机使能...";
    try {
        vector<uchar> data = { 0x02, 0x06, 0x00, 0x00, 0x01, 0x01, 0x49, 0xA9 };
        sendData(data);
        readData(8);
    } catch (const MyError& e) {
        qCritical() << e.desc();
        throw e;
    } catch (const exception& e) {
        qCritical() << "步进电机使能失败:" << e.what();
        qCritical() << m_port.errorString();
        throw MyError(MyErrorType::MOTO_ENABLING_ERROR);
    } catch (...) {
        qCritical() << "步进电机使能失败...";
        qCritical() << m_port.errorString();
        throw MyError(MyErrorType::MOTO_ENABLING_ERROR);
    }
}

void RS485Port::motorDisabling() {
    qInfo() << "电机失能...";
    try {
        vector<uchar> data = { 0x02, 0x06, 0x00, 0x00, 0x01, 0x00, 0x88, 0x69 };
        sendData(data);
        readData(8);
    } catch (const MyError& e) {
        qCritical() << e.desc();
        throw e;
    } catch (const exception& e) {
        qCritical() << "步进电机失能失败:" << e.what();
        qCritical() << m_port.errorString();
        throw MyError(MyErrorType::MOTO_DISABLING_ERROR);
    } catch (...) {
        qCritical() << "步进电机失能失败:";
        qCritical() << m_port.errorString();
        throw MyError(MyErrorType::MOTO_DISABLING_ERROR);
    }
}

void RS485Port::setMotoSpeed(float speed) {
    try {
        qInfo() << "设置运行速度：" << speed;
        unsigned char* hexVal = (unsigned char*)&speed;
        vector<uchar> data = { 0x02, 0x10, 0x00, 0x03, 0x00, 0x02, 0x04, hexVal[1], hexVal[0], hexVal[3], hexVal[2], 0x0F, 0x7D };
        //计算校验码
        ModBusCRC16(data);
        sendData(data);
        readData(8);
    } catch (const MyError& e) {
        qCritical() << e.desc();
        throw e;
    } catch (const exception& e) {
        qCritical() << "设置步进电机速度异常:" << e.what();
        qCritical() << m_port.errorString();
        throw MyError(MyErrorType::SET_MOTO_SPEED_ERROR);
    } catch (...) {
        qCritical() << "设置步进电机速度异常...";
        qCritical() << m_port.errorString();
        throw MyError(MyErrorType::SET_MOTO_SPEED_ERROR);
    }
}

void RS485Port::stopMotor() {
    try {
        qInfo() << "紧急停止电机...";
        vector<uchar> data = { 0x02, 0x06, 0x00, 0x00, 0x04, 0x01, 0x4A, 0xF9 };
        sendData(data);
        readData(8);
        //电机失能
        vector<uchar> data2 = { 0x02, 0x06, 0x00, 0x00, 0x01, 0x00, 0x88, 0x69 };
        sendData(data2);
        readData(8);
    } catch (const MyError& e) {
        qCritical() << e.desc();
        throw e;
    } catch (const exception& e) {
        qCritical() << "停止运动异常:" << e.what();
        qCritical() << m_port.errorString();
        throw MyError(MyErrorType::STOP_MOTO_ERROR);
    } catch (...) {
        qCritical() << "停止运动异常...";
        qCritical() << m_port.errorString();
        throw MyError(MyErrorType::STOP_MOTO_ERROR);
    }
}

int RS485Port::getMotorPos() {
    try {
        vector<uchar> data = { 0x02, 0x04, 0x00, 0x04, 0x00, 0x02, 0x30, 0x39 };
        if (m_printMsg) {
            qInfo() << "读取电机位置...";
        }
        sendData(data);
        QThread::msleep(100);
        //读取数据
        QByteArray buff = readData(9);
        int posVal = buff.at(4) & 0x03;
       // qDebug() << "原始值:" << int(buff.at(4)) << ", 掩码后值:" << posVal;
        //判断位置
        if (posVal == 0x02) {           //起始位置
            qDebug() << "pos: POS_RESET";
            return POS_RESET;
        } else if (posVal == 0x01) {    //末尾位置
            qDebug() << "pos: POS_TAIL";
            return POS_TAIL;
        }
    } catch (const MyError& e) {
        qCritical() << e.detail();
        throw e;
    } catch (const exception& e) {
        qCritical() << "获取相机位置异常:" << e.what();
        qCritical() << m_port.errorString();
        throw MyError(MyErrorType::GET_CAMERA_POSITION_ERROR);
    } catch (...) {
        qCritical() << "获取相机位置异常...";
        qCritical() << m_port.errorString();
        throw MyError(MyErrorType::GET_CAMERA_POSITION_ERROR);
    }
    //既不在末尾也不在起始位置
    return UNABLE_LOCATE;
}

void RS485Port::moveMoto(float speed, int step) {
    try {
        //设置运动模式
        initRunModel();
        //设置速度
        setMotoSpeed(speed);
        //电机使能
        motorEnabling();
        qInfo() << QString("移动%1步").arg(step);
        // 计算速度值，开始移动电机
        unsigned char* stepHexVal = (unsigned char*)&step;
        vector<uchar> data = { 0x02, 0x10, 0x00, 0x01, 0x00, 0x02, 0x04, stepHexVal[1], stepHexVal[0], stepHexVal[3], stepHexVal[2], 0x4E, 0x12 };
        ModBusCRC16(data);
        sendData(data);
        //读取驱动器返回的数据
        readData(8);
    } catch (const MyError& e) {
        qCritical() << e.detail();
        throw e;
    } catch (const exception& e) {
        qCritical() << "相机移动异常:" << e.what();
        qCritical() << m_port.errorString();
        throw MyError(MyErrorType::MOVE_CAMERA_ERROR);
    } catch (...) {
        qCritical() << "相机移动异常...";
        qCritical() << m_port.errorString();
        throw MyError(MyErrorType::MOVE_CAMERA_ERROR);
    }
}

void RS485Port::resetMoto(float speed, bool waitReset) {
    qDebug() << "#########################################################speed: " << speed;
    try {
        //判断电机位置
        if (getMotorPos() != POS_RESET) {
            //将平台复位
            moveMoto(speed, m_step);
        }
        //判断是否要等待复位完成再返回函数
        if (waitReset) {
            //等待电机复位
            while (getMotorPos() != POS_RESET) {
                QThread::msleep(100);
                QApplication::processEvents();
            }
            //stopMotor();
            //电机重新定位
            //向后移动500步
            moveMoto(10000, -5000);
            //等待移动结束
            qint64 beginTime = TimeUtils::getTimeGen();
            while (TimeUtils::getTimeGen() - beginTime < 500) {
                QThread::msleep(50);
                QApplication::processEvents();
            }
            //用最慢的速度返回
            moveMoto(2000, 6000);
            //保证复位完成
            while (getMotorPos() != POS_RESET) {
                QThread::msleep(50);
                QApplication::processEvents();
            }
        }
    } catch (const MyError& e) {
        qCritical() << e.detail();
        throw e;
    } catch (const exception& e) {
        qCritical() << "相机移动异常:" << e.what();
        qCritical() << m_port.errorString();
        throw MyError(MyErrorType::MOVE_CAMERA_ERROR);
    } catch (...) {
        qCritical() << "相机移动异常...";
        qCritical() << m_port.errorString();
        throw MyError(MyErrorType::MOVE_CAMERA_ERROR);
    }
}

// 接收一个回调函数和一个浮点数类型的速度参数
void RS485Port::moveMotoToTail(std::function<void()> fun, float speed) {
    qInfo() << QString("扫描运动...");
    try {
        //设置运动模式
        initRunModel();
        //设置速度
        setMotoSpeed(speed);
        qDebug() << "speed=" << speed;
        //电机使能
        motorEnabling();
        //计算速度值，开始移动电机
        int stepVal = -m_step;
        unsigned char* stepHexVal = (unsigned char*)&stepVal;
        vector<uchar> data = { 0x02, 0x10, 0x00, 0x01, 0x00, 0x02, 0x04, stepHexVal[1], stepHexVal[0], stepHexVal[3], stepHexVal[2], 0x4E, 0x12 };
        ModBusCRC16(data);
        sendData(data);
        //电机开始移动的时候执行传入的函数
        fun();
        //读取驱动器返回的数据
        // readData(8);
    } catch (const MyError& e) {
        qCritical() << e.detail();
        throw e;
    } catch (const exception& e) {
        qCritical() << "相机移动异常:" << e.what();
        qCritical() << m_port.errorString();
        throw MyError(MyErrorType::MOVE_CAMERA_ERROR);
    } catch (...) {
        qCritical() << "相机移动异常...";
        qCritical() << m_port.errorString();
        throw MyError(MyErrorType::MOVE_CAMERA_ERROR);
    }
}

void RS485Port::ledOn(int num) {
    try {
        //顶灯
        if (num & SLIDER) {
            vector<uchar> data = { 0x01, 0x06, 0x00, 0x00, 0x00, 0x01, 0x48, 0x0A };
            sendData(data);
            readData(8);
        }
        //开启侧灯
        if (num & BOTTOM) {
            vector<uchar> data = { 0x01, 0x06, 0x00, 0x01, 0x00, 0x01, 0x19, 0xCA };
            sendData(data);
            readData(8);
        }
        //开启底灯
        if (num & TOP) {
            vector<uchar> data = { 0x01, 0x06, 0x00, 0x02, 0x00, 0x01, 0xE9, 0xCA };
            sendData(data);
            readData(8);
        }
    } catch (const MyError& e) {
        qCritical() << e.detail();
        throw e;
    } catch (const exception& e) {
        qCritical() << "光源控制异常:" << e.what();
        qCritical() << m_port.errorString();
        throw MyError(MyErrorType::LED_SWITCH_ERROR);
    } catch (...) {
        qCritical() << "光源控制异常...";
        qCritical() << m_port.errorString();
        throw MyError(MyErrorType::LED_SWITCH_ERROR);
    }
}

void RS485Port::ledOff(int num) {
    try {
        //顶灯
        if (num & SLIDER) {
            vector<uchar> data = { 0x01, 0x06, 0x00, 0x00, 0x00, 0x00, 0x89, 0xCA };
            sendData(data);
            readData(8);
        }
        //开启侧灯
        if (num & BOTTOM) {
            vector<uchar> data = { 0x01, 0x06, 0x00, 0x01, 0x00, 0x00, 0xD8, 0x0A };
            sendData(data);
            readData(8);
        }
        //开启底灯
        if (num & TOP) {
            vector<uchar> data = { 0x01, 0x06, 0x00, 0x02, 0x00, 0x00, 0x28, 0x0A };
            sendData(data);
            readData(8);
        }
    } catch (const MyError& e) {
        qCritical() << e.detail();
        throw e;
    } catch (const exception& e) {
        qCritical() << "光源控制异常:" << e.what();
        qCritical() << m_port.errorString();
        throw MyError(MyErrorType::LED_SWITCH_ERROR);
    } catch (...) {
        qCritical() << "光源控制异常：" << m_port.errorString();
        throw MyError(MyErrorType::LED_SWITCH_ERROR);
    }
}

QByteArray RS485Port::readData(int byteCount, int timeout) {
    QByteArray buff;
    int64 beginTime = TimeUtils::getTimeGen();
    try {
        while (buff.size() < byteCount && (TimeUtils::getTimeGen() - beginTime) < timeout) {
            if (m_port.bytesAvailable() > 0) {
                buff.push_back(m_port.read(1)[0]);
            } else {
                m_port.waitForReadyRead(5);
            }
        }
        if (m_printMsg) {
            qInfo() << "receive:" << buff.toHex(' ');
        }
        if (buff.size() != byteCount) {
            buff.clear();
            m_port.clear();
            throw MyError(MyErrorType::SERIAL_READ_ERROR);
        }
    } catch (const std::exception& e) {
        qCritical() << "读数据异常:" << e.what();
        qCritical() << m_port.errorString();
        throw MyError(MyErrorType::SERIAL_READ_ERROR);
    } catch (...) {
        qCritical() << "读数据异常:";
        qCritical() << m_port.errorString();
        throw MyError(MyErrorType::SERIAL_READ_ERROR);
    }
    return buff;
}


//////////////////////////////////////////////////////////////////////////
///                            RS232Port                               ///
//////////////////////////////////////////////////////////////////////////


RS232Port::RS232Port(QObject* parent) : SerialPort(parent) {
    connect(&m_port, &QSerialPort::readyRead, this, &RS232Port::onDataArrive);
}

void RS232Port::onDataArrive() {
    QByteArray buff;
    while (m_port.bytesAvailable() > 0) {
        QByteArray data = m_port.read(1);
        buff.push_back(data[0]);
    }
    qDebug() << "receive:" << buff.toHex(' ');
}
